#include "laser_node.h"

// This function must be implemented in the .cpp file to create
// a plugin that can be loaded at run-time
BT_REGISTER_NODES(factory)
{
    LaserNode::RegisterNodes(factory);
}

inline void SleepMS(int ms)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}

static bool isStartSignal = false;
constexpr static unsigned int checkStand = 250;

NodeStatus LaserNode::IsReceiveStartSignal()
{
    static unsigned int checkCont = 0;
    checkCont++;
    SleepMS(200);
    if(checkCont >= 60)
    {
        isStartSignal = true;
    }
    return isStartSignal? NodeStatus::SUCCESS : NodeStatus::FAILURE;
}


NodeStatus LaserNode::LaserSpaceJudg::tick()
{
    unsigned int space = 0;
    getInput("space", space);
    if(space >= checkStand)
    {
        return NodeStatus::SUCCESS;
    }
    return NodeStatus::FAILURE;
}

// Register at once all the Actions and Conditions in this file
void LaserNode::RegisterNodes(BehaviorTreeFactory& factory)
{
    factory.registerSimpleAction("Laser_isReceiveStartSignal", std::bind(IsReceiveStartSignal));
    factory.registerNodeType<LaserSpaceJudg>("Laser_isLaserSpaceJudg");
    factory.registerNodeType<AlarmNode>("Laser_Alarm");
    factory.registerNodeType<AxisMoveToNode>("Laser_AxisMoveTo");
    factory.registerNodeType<ProcessNode>("Laser_process");
}

